<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="UTF-8" />
    <title>Document</title>
    <script type="text/javascript" src="http://cdn.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
    <script type="text/javascript" src="http://cdn.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
</head>
<body>
<!--<img id="tulip" src="images/small.jpg" alt="The Tulip" style="display:none;"/>-->
<canvas width="500" height="500" id="c1"></canvas>
</body>
</html>
<script type="text/javascript">
    window.onload = function(){
        var oC = document.getElementById('c1');
        var oGC = oC.getContext("2d");
        oC.style.backgroundColor = 'black';
        var oldDir = {x:0,y:0};
        var zeroDir = {x:0,y:0};
        var startX = 0;
        var startY = 0;
        // Connecting to ROS
        // -----------------
        var ros = new ROSLIB.Ros({
            url : 'ws://192.168.100.53:9090'
        });

        ros.on('connection', function() {
            console.log('Connected to websocket server.');
        });

        ros.on('error', function(error) {
            console.log('Error connecting to websocket server: ', error);
        });

        ros.on('close', function() {
            console.log('Connection to websocket server closed.');
        });

        // Publishing a Topic
        // ------------------

        var cmdVel = new ROSLIB.Topic({
            ros : ros,
            name : '/cmd_vel',
            messageType : 'geometry_msgs/Twist'
        });

        var twist = new ROSLIB.Message({
            linear : {
                x : 10,
                y : 0,
                z : 0
            },
            angular : {
                x : 0,
                y : 0,
                z : 10
            }
        });

        var timer = null;
        setInterval(function(){
            cmdVel.publish(twist);
            console.log("publish to cmdVel");
        },500);
        // Subscribing to a Topic
        // ----------------------

        var listener = new ROSLIB.Topic({
            ros : ros,
            name : '/amcl_pose',
            messageType : 'geometry_msgs/PoseWithCovarianceStamped'
        });

        listener.subscribe(function(message) {
//            console.log('Received message on ' + listener.name + ': ' + message.header+ ': ' +message.pose);
            console.log(message.pose.pose.position);
            startX = message.pose.pose.position.x;
            startY = message.pose.pose.position.y;
            drawDir(startX,startY);

            drawLine(oldDir.x,oldDir.y,message.pose.pose.position.x,message.pose.pose.position.y);
            oldDir = {
                x:message.pose.pose.position.x,
                y:message.pose.pose.position.y
            }
            //listener.unsubscribe();
        });

        // Calling a service
        // -----------------

        var addTwoIntsClient = new ROSLIB.Service({
            ros : ros,
            name : '/add_two_ints',
            serviceType : 'rospy_tutorials/AddTwoInts'
        });

        var request = new ROSLIB.ServiceRequest({
            a : 1,
            b : 2
        });

        addTwoIntsClient.callService(request, function(result) {
            console.log('Result for service call on '
                + addTwoIntsClient.name
                + ': '
                + result.sum);
        });

        // Getting and setting a param value
        // ---------------------------------

        ros.getParams(function(params) {
            console.log(params);
        });

        var maxVelX = new ROSLIB.Param({
            ros : ros,
            name : 'max_vel_y'
        });

        maxVelX.set(0.8);
        maxVelX.get(function(value) {
            console.log('MAX VAL: ' + value);
        });
        function drawDir(startX,startY){
            oGC.save();
            oGC.fillStyle = 'red';
            oGC.beginPath();
            oGC.moveTo(startX+zeroDir.x, startY+zeroDir.y);
            oGC.arc(startX+zeroDir.x,startY+zeroDir.y,2,0,360*Math.PI/180,false);
            oGC.closePath();
            oGC.globalCompositeOperation="source-over";
            oGC.fill();
            oGC.restore();
        }
        function drawLine(x0,y0,x1,y1){
            oGC.save();
            oGC.strokeStyle="red"
            oGC.beginPath();
            oGC.moveTo(x0+zeroDir.x , y0+zeroDir.y);
            oGC.lineTo(x1+zeroDir.x, y1+zeroDir.y);
            oGC.stroke();
            oGC.restore();

        }

    }
</script>